#include "rclcpp/rclcpp.hpp"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl/io/pcd_io.h"
#include "pcl/filters/passthrough.h"

using PointT = pcl::PointXYZI;

class PcdFilterNode : public rclcpp::Node
{
public:
    PcdFilterNode() : Node("pcd_filter_node")
    {
        // --- 1. 声明ROS参数，这样我们可以从命令行传入配置 ---
        this->declare_parameter<std::string>("input_file", "/home/jingzedong/B2_NAV_ws/map/map_real/cloudGlobal.pcd");
        this->declare_parameter<std::string>("output_file", "/home/jingzedong/B2_NAV_ws/src/hdl_Localization_ROS2/hdl_localization/data/80.pcd");
        this->declare_parameter<double>("intensity_threshold", 80.0);
        this->declare_parameter<double>("height_threshold", -0.5);

        // 立即执行过滤任务
        run_filter();
    }

private:
    void run_filter()
    {
        // --- 2. 获取参数值 ---
        std::string input_path = this->get_parameter("input_file").as_string();
        std::string output_path = this->get_parameter("output_file").as_string();
        float intensity_thresh = this->get_parameter("intensity_threshold").as_double();
        float height_thresh = this->get_parameter("height_threshold").as_double();

        if (input_path.empty() || output_path.empty())
        {
            RCLCPP_ERROR(this->get_logger(), "输入或输出文件路径不能为空！请通过参数设置。");
            rclcpp::shutdown();
            return;
        }

        RCLCPP_INFO(this->get_logger(), "开始处理文件: %s", input_path.c_str());

        // --- 3. 加载点云 ---
        pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
        if (pcl::io::loadPCDFile<PointT>(input_path, *cloud) == -1)
        {
            RCLCPP_ERROR(this->get_logger(), "无法加载PCD文件: %s", input_path.c_str());
            rclcpp::shutdown();
            return;
        }
        RCLCPP_INFO(this->get_logger(), "成功加载点云，包含 %ld 个点。", cloud->size());

        // --- 4. 应用高度滤波器 (Z轴) ---
        RCLCPP_INFO(this->get_logger(), "正在应用高度滤波器...");
        pcl::PointCloud<PointT>::Ptr cloud_after_z(new pcl::PointCloud<PointT>);
        pcl::PassThrough<PointT> pass_z;
        pass_z.setInputCloud(cloud);
        pass_z.setFilterFieldName("z");

        float height_min = 0.0;
        float height_max = 1.5;
        pass_z.setFilterLimits(height_min, height_max);
        // ==========================================================

        pass_z.filter(*cloud_after_z);
        RCLCPP_INFO(this->get_logger(), "保留 Z 轴范围在 [%.2f, %.2f] 米的点后，剩余 %ld 个点。", height_min, height_max, cloud_after_z->size());

        // --- 5. 应用强度滤波器 ---
        pcl::PointCloud<PointT>::Ptr final_cloud(new pcl::PointCloud<PointT>);
        pcl::PassThrough<PointT> pass_i;
        pass_i.setInputCloud(cloud_after_z);
        pass_i.setFilterFieldName("intensity");
        pass_i.setFilterLimits(intensity_thresh, std::numeric_limits<float>::max());
        pass_i.filter(*final_cloud);
        RCLCPP_INFO(this->get_logger(), "应用强度滤波器 (intensity >= %.2f) 后，最终剩余 %ld 个点。", intensity_thresh, final_cloud->size());

        // --- 6. 保存过滤后的点云 ---
        RCLCPP_INFO(this->get_logger(), "正在保存过滤后的点云至: %s", output_path.c_str());
        pcl::io::savePCDFileBinary(output_path, *final_cloud);
        RCLCPP_INFO(this->get_logger(), "处理完成！");

        // 任务完成，关闭节点
        rclcpp::shutdown();
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<PcdFilterNode>();
    // 由于节点在构造函数中执行完任务后会调用 shutdown，所以我们不需要 spin
    return 0;
}